
//#include "Filter_match_keypoint.h"
//#include "orbMatch.h"
#include "Filter_match_keypoint.h"

#include "orbMatch.h"

static Mat _DrawMatchImage(const Mat &img1, vector<cv::KeyPoint> kp1, const Mat &img2, vector<cv::KeyPoint> kp2);



int orbMatch(cv::Mat& in_src_img, cv::Mat& in_dst_img, std::vector<cv::Point2f>& outPoint1, std::vector<cv::Point2f>& outPoint2, struct orbMatch orbArg, 
            float goodmatch_val1, float goodmatch_val2, float goodmatch_val3)
{
//initial
	std::vector<KeyPoint> keypoints_1, keypoints_2;
	cv::Mat descriptors_1, descriptors_2;


    Point2f p1_temp, p2_temp;

    std::vector<DMatch> matches;    //DMatch是用来描述匹配好的一对特征点的类，包含这两个点之间的匹配信息
                            //比如左图有个特征m，它和右图的特征点n最匹配，这个DMatch就记录它俩最匹配，并且还记录m和n的
                            //特征向量的距离和其他信息，这个距离在后面用来做筛选 
	std::vector<DMatch> goodmatches_temp;
    vector<DMatch> good_matcher_V2;

	//>>>>ORB
	Ptr<ORB> orb = ORB::create(500, 1.8f, 8, 31, 0, 4, ORB::HARRIS_SCORE, 31, 20); //  <<<<<<<<<<<<<<<<<<<<<<<

	//computer keypoint and descriptors
	orb->detectAndCompute(in_src_img, Mat(), keypoints_1, descriptors_1);
	orb->detectAndCompute(in_dst_img, Mat(), keypoints_2, descriptors_2);
		printf("img1 point all num is: %d \n", (int)keypoints_1.size());
		printf("img2 point all num is: %d \n", (int)keypoints_2.size());

    //cout << "keypoints_1: " << keypoints_1.size() << endl;
	if(orbArg.isShowKeyPoints ){
		Mat img_keypoint;
        cv::drawKeypoints(in_src_img,keypoints_1, img_keypoint,Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
        cvNamedWindow("BFMatcher img_keypointImage", 0);
        imshow("BFMatcher img_keypointImage", img_keypoint);
        cv::imwrite("./orbMatch_img_keypointImage.jpg", img_keypoint);
        waitKey(0);
        cvDestroyAllWindows();
	}
    

    double min_dist = 10000, max_dist = 0;
	if (orbArg.method == "flannmatch") {
		//FLANN匹配
        FlannBasedMatcher matcherFlann;         //实例化一个FLANN匹配器(括号里可以选择匹配方法)
        matcherFlann.match(descriptors_1, descriptors_2, matches);             //匹配，数据来源是特征向量，结果存放在DMatch类型里面  

        //求最小最大距离
        for (int i=0; i< descriptors_1.rows; i++) {
            double distance = matches[i].distance;
            if (distance > max_dist){
                max_dist = distance;
            }
            if (distance < min_dist){ 
                min_dist = distance;
            }
        }
        //printf("max distance : %f\n", max_dist);
        //printf("min distance : %f\n", min_dist);

        //筛选较好的匹配点
        for (int i = 0; i < descriptors_1.rows; i++) {
            double distance = matches[i].distance;
            if (distance < max(min_dist * 2, 0.02)) {
                goodmatches_temp.push_back(matches[i]);//距离小于范围的压入新的DMatch
            }
        }
    }
    else if(orbArg.method == "bfmatch"){

        //BFMatcher matcherBf(NORM_L2);         //实例化一个暴力匹配器(括号里可以选择匹配方法)            
        BFMatcher matcherBf(NORM_HAMMING);
        matcherBf.match(descriptors_1, descriptors_2, matches);   //匹配，数据来源是特征向量，结果存放在DMatch类型里面  
        //匹配点筛选
        //sort函数对数据进行升序排列
        //筛选匹配点，根据match里面特征对的距离从小到大排序    

        sort(matches.begin(), matches.end());  
        cout << "matches size: " << matches.size() << endl;  
//No.4 filter matche_points
		for (int i = 0; i < descriptors_1.rows; i++){
			double dist = matches[i].distance;
			if (dist < min_dist)min_dist = dist;
			if (dist > max_dist)max_dist = dist;
		}
		//DEBUG_PRINT("--max dist: %f \n", max_dist);
		//DEBUG_PRINT("--min dist: %f \n", min_dist);
        printf("bfmatch: >>>> \n");
        printf("match: %d,  " , descriptors_1.rows);

		for (int i = 0; i < descriptors_1.rows; i++){
			if (1 | matches[i].distance <= max(float(goodmatch_val1 * min_dist), goodmatch_val2)){
				goodmatches_temp.push_back(matches[i]);
			}

		}
		//DEBUG_PRINT( "matches number: %d \n", (int)matches.size());
		//DEBUG_PRINT("goodmatches number: %d \n", (int)goodmatches_temp.size());
   
    }
    else {
		cout << "no this method" << endl;
		return -1;
	
	}

    printf("max distance : %f\n", max_dist);
    printf("min distance : %f\n", min_dist);
    cout << "good keypoint temp: " << goodmatches_temp.size() << endl;
	
    //5.remove good_keypoints to new container-point1,2
    for (size_t i = 0; i < goodmatches_temp.size(); i++){
        p1_temp = keypoints_1[goodmatches_temp[i].queryIdx].pt;
        p2_temp = keypoints_2[goodmatches_temp[i].trainIdx].pt;

        const float tt = point_danstence(p1_temp, p2_temp);
        if (tt > goodmatch_val3) continue;
        
        good_matcher_V2.push_back(goodmatches_temp[i]);
        outPoint1.push_back(p1_temp);
        outPoint2.push_back(p2_temp);

    }
    if (good_matcher_V2.size() < 10){
        cerr << "good matchers is too less!!!    " << good_matcher_V2.size() << endl;
        #if 0
        //No.5 draw the result of matching
        namedWindow("errors_img" , 0);
        imshow("errors_img", img_goodmatch);
        waitKey(0);
        #endif
        return -1;
    }
    cout << "good keypoint out: " << good_matcher_V2.size() << endl;
    //<<<<
    // if (orbArg.isShowKeyPoints){
    //     Mat img_keypoint;
    //     cv::drawKeypoints(in_src_img,keypoints_1, img_keypoint);
    //     cvNamedWindow("BFMatcher img_keypointImage", 0);
    //     imshow("BFMatcher img_keypointImage", img_keypoint);
    //     cv::imwrite("./img_keypointImage", img_keypoint);
    //     waitKey(0);
    //     cvDestroyAllWindows();
    // }
    if (orbArg.isShowMatchImage){
        Mat img_goodmatch;
        cv::drawMatches(in_src_img, keypoints_1, in_dst_img, keypoints_2, good_matcher_V2, img_goodmatch, cv::Scalar(0,255,0));

        //img_goodmatch = _DrawMatchImage(in_src_img, keypoints_1, in_dst_img, keypoints_2);
        cvNamedWindow("BFMatcher Image", 0);
        imshow("BFMatcher Image", img_goodmatch);
        cv::imwrite("./orbMatch_img_matchImage.jpg", img_goodmatch);
        waitKey(0);
        cvDestroyAllWindows();
    }

	return 0;

}


static Mat _DrawMatchImage(const Mat &img1, vector<cv::KeyPoint> kp1, const Mat &img2, vector<cv::KeyPoint> kp2)
{
	if(img1.channels() == 3){
		cout << "OrbProcess.cpp :: can not process bgr_img" << endl;
		exit(0);
	}
	if(kp1.size() != kp2.size()){
		cout << "OrbProcess.cpp :: this two images'keypoint_size is not equit, kpq: " << kp1.size() << "  kp2: " << kp2.size() << endl;
		return Mat();
	}
	int row = img1.rows;
	int col = img1.cols;
	Mat outputImg = Mat::zeros(row, col * 2, img1.type());
	cv::Rect rect1 = cv::Rect(0,0,col, row);  //(x,y,w,h)
	cv::Rect rect2 = cv::Rect(col, 0, col, row);

	Mat roi = outputImg(rect1);
	img1.colRange(0,col).copyTo(roi);
	roi = outputImg(rect2);
	img2.colRange(0, col).copyTo(roi);
	
	Mat bgr_output(outputImg.size(), CV_8UC3);
	cvtColor(outputImg, bgr_output, COLOR_GRAY2BGR);

	Point2f p1, p2;
	vector<cv::KeyPoint>::iterator it1;
	vector<cv::KeyPoint>::iterator it2;
	for (it1 = kp1.begin(), it2 = kp2.begin(); it1 != kp1.end(); it1++, it2++){
		p1 = (*it1).pt;
		p2 = (*it2).pt;
		p2.x += col;
		line (bgr_output, p1, p2, Scalar(0,255,0), 2);
	
	}
	rectangle(bgr_output, Point(0,0), Point(200,200), Scalar(0,0,255), 2, 8, 0);


	namedWindow("ORB img_goodmatch", 0);
	cv::imshow("ORB img_goodmatch", bgr_output);
	cv::imwrite("./imgGoodMatch.jpg", bgr_output);
	cv::waitKey(0);
	
	return bgr_output;

}
